package org.hitlabnz.sensor_fusion_demo.orientationProvider;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorManager;
import android.util.Log;

public class AccelerometerProvider extends OrientationProvider {

    /**
     * Compass values
     */
    final private float[] magnitudeValues = new float[3];

    /**
     * Accelerometer values
     */
    final private float[] accelerometerValues = new float[3];

    /**
     * Inclination values
     */
    final float[] inclinationValues = new float[16];

    /**
     * Initialises a new OrientationProvider
     *
     * @param sensorManager The android sensor manager
     */
    public AccelerometerProvider(SensorManager sensorManager) {
        super(sensorManager);

        //Add the accelerometer
        sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER));
    }

    private float[] orientation = new float[3];

    @Override
    public void onSensorChanged(SensorEvent event) {
        // we received a sensor event. it is a good practice to check
        // that we received the proper event
        if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            System.arraycopy(event.values, 0, accelerometerValues, 0, accelerometerValues.length);
        }

        if (accelerometerValues != null) {
            // calculate angles with accelerometer
            orientation[0] = (float) Math.atan2(accelerometerValues[0], Math.sqrt(accelerometerValues[1] * accelerometerValues[1] + accelerometerValues[2] * accelerometerValues[2]));
            orientation[1] = (float) Math.atan2(accelerometerValues[1], Math.sqrt(accelerometerValues[0] * accelerometerValues[0] + accelerometerValues[2] * accelerometerValues[2]));
            orientation[2] = (float) Math.atan2(accelerometerValues[2], Math.sqrt(accelerometerValues[1] * accelerometerValues[1] + accelerometerValues[0] * accelerometerValues[0]));
            // convert to degree
            orientation[0] = (float) Math.toDegrees(orientation[0]);
            orientation[1] = (float) Math.toDegrees(orientation[1]);
            orientation[2] = (float) Math.toDegrees(orientation[2]);
            Log.d("AccelerometerProvider", "X=" + orientation[0]);
            Log.d("AccelerometerProvider", "Y=" + orientation[1]);
            Log.d("AccelerometerProvider", "Z=" + orientation[2]);
            currentOrientationQuaternion.setEulerAngle(orientation[2], orientation[1], orientation[0]);
        }
    }


}
